// ///////////////////////////// MIT License //////////////////////////////////// //
//                                                                                //
// Copyright (c) 2010 David Zsolt Manrique                                        //
//                    david.zsolt.manrique@gmail.com                              //
//                                                                                //
// Permission is hereby granted, free of charge, to any person obtaining a copy   //
// of this software and associated documentation files (the "Software"), to deal  //
// in the Software without restriction, including without limitation the rights   //
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell      //
// copies of the Software, and to permit persons to whom the Software is          //
// furnished to do so, subject to the following conditions:                       //
//                                                                                //
// The above copyright notice and this permission notice shall be included in     //
// all copies or substantial portions of the Software.                            //
//                                                                                //
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR     //
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,       //
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE    //
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER         //
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,  //
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN      //
// THE SOFTWARE.                                                                  //
//                                                                                //
// ////////////////////////////////////////////////////////////////////////////// //

// ////////////////////////////////////////////////////////////////////////////// //
// Reference:                                                                     //
// Please be aware that the force filed forms are intend to be compatible with    //
// DL_POLY (http://www.cse.scitech.ac.uk/ccg/software/DL_POLY/)                   //
// and the inversion forces for testing purpose are based on                      //
// the source code of DL_POLY. However it is not part of this softwer!            //
//                                                                                //
// Please visit the site http://www.cse.scitech.ac.uk/ccg/software/DL_POLY/       //
// ////////////////////////////////////////////////////////////////////////////// //


#include "potential.h"

void background::constant::eval(real_type & u,vector_array_type & f,const vector_array_type & r) const
{
	u += p_u0;
}
void background::constant_fit::peval(real_type & u,vectorx_type & dp,const vectorx_type & p,const vector_array_type & r)
{
	if(i_u0>=0)  p_u0  = p(i_u0);
	u += p_u0;
	if(i_u0>=0)  dp(i_u0)  += 1.0;
}


void bond::harm::eval(real_type & u,vector_array_type & f,const vector_array_type & r) const
{
	vector_type rij = r[j]-r[i];
	real_type   dij = rij.norm();

	vector_type fi;

	u += 0.5*p_k*function::sqr(dij-p_r0);

	fi = p_k*(dij-p_r0)*rij/dij;

	f[i] += fi;
	f[j] += -fi;

}


void bond::harm_fit::pdiff(real_type & u,vectorx_type & dp,const vectorx_type & p,const vector_array_type & r)
{throw;}
void bond::harm_fit::peval(real_type & u,vectorx_type & dp,const vectorx_type & p,const vector_array_type & r)
{
	if(i_k>=0)  p_k  = p(i_k);
	if(i_r0>=0) p_r0 = p(i_r0);

	vector_type rij = r[j]-r[i];
	real_type   dij = rij.norm();

	u += 0.5*p_k*function::sqr(dij-p_r0);
	if(i_k>=0)  dp(i_k)  += 0.5*function::sqr(dij-p_r0);
	if(i_r0>=0) dp(i_r0) += -p_k*(dij-p_r0);
}
void bond::mors::eval(real_type & u,vector_array_type & f,const vector_array_type & r) const
{
	vector_type a = r[j]-r[i];
	real_type   la = a.norm();
	
	vector_type fi;
	
	real_type ex = std::exp(-p_k*(la-p_r0));
	u += p_E0*function::sqr(1.0-ex);
	real_type du = 2.0*p_E0*p_k*ex*(1.0-ex);

	fi = du*a/la;
	f[i] += fi;
	f[j] += -fi;

}


void bond::mors_fit::pdiff(real_type & u,vectorx_type & dp,const vectorx_type & p,const vector_array_type & r)
{ throw; }
void bond::mors_fit::peval(real_type & u,vectorx_type & dp,const vectorx_type & p,const vector_array_type & r)
{

	if(i_E0>=0)  p_E0  = p(i_E0);
	if(i_k >=0)  p_k   = p(i_k);
	if(i_r0>=0)  p_r0  = p(i_r0);

	// /////////////////////////////// //
	vector_type a = r[j]-r[i];
	real_type   la = a.norm();
	real_type ex = std::exp(-p_k*(la-p_r0));
	u += p_E0*function::sqr(1.0-ex);  
	// /////////////////////////////// //

	if(i_E0>=0)  dp(i_E0)  +=  function::sqr(1.0-ex);
	if(i_k >=0)  dp(i_k)   +=  2.0*p_E0*(la-p_r0)*ex*(1.0-ex);
	if(i_r0>=0)  dp(i_r0)  += -2.0*p_E0*p_k*ex*(1.0-ex);
}



void angle::harm::eval(real_type & u,vector_array_type & f,const vector_array_type & r) const
{
	vector_type rij  = r[j]-r[i];
	vector_type rjk  = r[k]-r[j];
	real_type   dij  = rij.norm();
	real_type   djk  = rjk.norm();
	vector_type uij  = rij/dij;
	vector_type ujk  = rjk/djk;
	real_type   cosphi = -function::cosa(rij,rjk);
	real_type   phi = std::acos(cosphi);
	real_type   sinphi = std::sin(phi);

	double c = -p_k*(phi-p_phi0)/sinphi;
	vector_type fi = -c*(ujk+cosphi*uij)/dij;
	vector_type fk = c*(uij+cosphi*ujk)/djk;
	vector_type fj = -(fi+fk);

	u += 0.5*p_k*function::sqr(phi-p_phi0);
	
	f[i] += fi;
	f[j] += fj;
	f[k] += fk;

}
	
void angle::harm_fit::pdiff(real_type & u,vectorx_type & dp,const vectorx_type & p,const vector_array_type & r)
{throw;}

void angle::harm_fit::peval(real_type & u,vectorx_type & dp,const vectorx_type & p,const vector_array_type & r)
{
	if(i_k>=0)		p_k    = p(i_k);
	if(i_phi0>=0) p_phi0 = p(i_phi0);

	vector_type a  = r[i]-r[j];
	vector_type b  = r[k]-r[j];
	real_type   la  = a.norm();
	real_type   lb  = b.norm();
	real_type   cosphi = a.dot(b)/la/lb;
	real_type   phi = std::acos(cosphi);
	
	u += 0.5*p_k*function::sqr(phi-p_phi0);

	if(i_k>=0)    dp(i_k)    += 0.5*function::sqr(phi-p_phi0);
	if(i_phi0>=0) dp(i_phi0) += -p_k*(phi-p_phi0);
}



void angle::hcos::eval(real_type & u,vector_array_type & f,const vector_array_type & r) const
{
	vector_type rij  = r[j]-r[i];
	vector_type rjk  = r[k]-r[j];
	real_type   dij  = rij.norm();
	real_type   djk  = rjk.norm();
	vector_type uij  = rij/dij;
	vector_type ujk  = rjk/djk;
	real_type   cosphi = -function::cosa(rij,rjk);

	double du = p_k*(cosphi-p_cosphi0);
	vector_type fi = -du*(ujk+cosphi*uij)/dij;
	vector_type fk = du*(uij+cosphi*ujk)/djk;
	vector_type fj = -(fi+fk);

	u += 0.5*p_k*function::sqr(cosphi-p_cosphi0);
	
	f[i] += fi;
	f[j] += fj;
	f[k] += fk;

}
	
void angle::hcos_fit::pdiff(real_type & u,vectorx_type & dp,const vectorx_type & p,const vector_array_type & r)
{throw;}

void angle::hcos_fit::peval(real_type & u,vectorx_type & dp,const vectorx_type & p,const vector_array_type & r)
{
	if(i_k>=0)		p_k    = p(i_k);
	if(i_cosphi0>=0) p_cosphi0 = p(i_cosphi0);

	vector_type rij  = r[j]-r[i];
	vector_type rjk  = r[k]-r[j];
	real_type   dij  = rij.norm();
	real_type   djk  = rjk.norm();
	vector_type uij  = rij/dij;
	vector_type ujk  = rjk/djk;
	real_type   cosphi = -function::cosa(rij,rjk); //std::cerr << "nnn"<< std::endl;
	u += 0.5*p_k*function::sqr(cosphi-p_cosphi0);
	
	if(i_k>=0)    dp(i_k)    += 0.5*function::sqr(cosphi-p_cosphi0);
	if(i_cosphi0>=0) dp(i_cosphi0) += -p_k*(cosphi-p_cosphi0);
}



void dihedral::harm::eval(real_type & u,vector_array_type & f,const vector_array_type & r) const
{
    vector_type a = r[i] - r[j];  vector_type b = r[j] - r[k];  vector_type c = r[k] - r[l];

    real_type aa = a.dot(a);  real_type bb = b.dot(b);  real_type cc = c.dot(c);
    real_type ab = a.dot(b);  real_type ac = a.dot(c);  real_type bc = b.dot(c);
    
    vector_type axb   = a.cross(b);       vector_type bxc   = b.cross(c); 

    real_type   ilaxb = 1.0/axb.norm();   real_type   ilbxc = 1.0/bxc.norm();
    
    //axbbxc = axb*bxc;
    real_type axbbxc = ab * bc - bb * ac;
    
    real_type inv = ilaxb*ilbxc;
    
    vector_type Aaxbbxc = b * bc - bb * c;
    vector_type Baxbbxc = a * bc + ab * c - 2.0 * b * ac;
    vector_type Caxbbxc = ab * b - bb * a;
    
    vector_type Alaxb = ilaxb * ( bb * a - ab * b );
    vector_type Blaxb = ilaxb * ( aa * b - ab * a );

    vector_type Blbxc = ilbxc * ( cc * b - bc * c );
    vector_type Clbxc = ilbxc * ( bb * c - bc * b );

    vector_type Ailaxb = - ilaxb*ilaxb * Alaxb;
    vector_type Bilaxb = - ilaxb*ilaxb * Blaxb;    
    
    vector_type Bilbxc = - ilbxc*ilbxc * Blbxc;
    vector_type Cilbxc = - ilbxc*ilbxc * Clbxc;
        
    vector_type Ainv = ilbxc * Ailaxb;
    vector_type Binv = Bilaxb * ilbxc + ilaxb * Bilbxc;
    vector_type Cinv = ilaxb * Cilbxc;
            
    real_type cosphi = axbbxc*inv;
    
    vector_type Acosphi = Aaxbbxc * inv + axbbxc * Ainv;
    vector_type Bcosphi = Baxbbxc * inv + axbbxc * Binv;
    vector_type Ccosphi = Caxbbxc * inv + axbbxc * Cinv;
    
    real_type phi = acos(cosphi);
    real_type isinphi = 1.0/sin(phi);
 
	/////////////
    u += 0.5*p_k*function::sqr(phi-p_phi0);
    real_type du = p_k*(phi-p_phi0);
    /////////////

    vector_type fi = isinphi * du * Acosphi;
    vector_type fj = isinphi * du * (Bcosphi - Acosphi);
    vector_type fl = -isinphi * du * Ccosphi;
    vector_type fk = -fi-fj-fl;

	f[i] += fi;
	f[j] += fj;
	f[k] += fk;
	f[l] += fl;

}
	

void dihedral::harm_fit::peval(real_type & u,vectorx_type & dp,const vectorx_type & p,const vector_array_type & r)
{
	if(i_k>=0)		p_k    = p(i_k);
	if(i_phi0>=0) p_phi0 = p(i_phi0);

	real_type   cosphi = (r[i] - r[j]).cross(r[j] - r[k]).normalized().dot((r[j] - r[k]).cross(r[k] - r[l]).normalized());
	real_type   phi = std::acos(cosphi);
	u += 0.5*p_k*function::sqr(phi-p_phi0);
	if(i_k>=0)    dp(i_k)    += 0.5*function::sqr(phi-p_phi0);
	if(i_phi0>=0) dp(i_phi0) += -p_k*(phi-p_phi0);
}



	


void inversion_plan_force(double A,

double xa,double ya,double za,
double xb,double yb,double zb,
double xc,double yc,double zc,
double xd,double yd,double zd,

double & u,

double &fax,double &fay,double &faz,
double &fbx,double &fby,double &fbz,
double &fcx,double &fcy,double &fcz,
double &fdx,double &fdy,double &fdz			
) 
{

	double xab= xb - xa;
	double yab= yb - ya;
	double zab= zb - za;
	double rab2=xab*xab+yab*yab+zab*zab;
	double rrab=1.0/sqrt(rab2);

	double xac= xc - xa;
	double yac= yc - ya;
	double zac= zc - za;
	double rac2=xac*xac+yac*yac+zac*zac;
	double rrac=1.0/sqrt(rac2);

	double xad= xd - xa;
	double yad= yd - ya;
	double zad= zd - za;
	double rad2=xad*xad+yad*yad+zad*zad;
	double rrad=1.0/sqrt(rad2);

	double rbc=xab*xac+yab*yac+zab*zac;
	double rcd=xac*xad+yac*yad+zac*zad;
	double rdb=xad*xab+yad*yab+zad*zab;

	// calculate bond-angle-plane vectors

	double ubx=xac*rrac+xad*rrad;
	double uby=yac*rrac+yad*rrad;
	double ubz=zac*rrac+zad*rrad;
	double ubn=1.0/sqrt(ubx*ubx+uby*uby+ubz*ubz);
	ubx=ubn*ubx;
	uby=ubn*uby;
	ubz=ubn*ubz;
	double rub=xab*ubx+yab*uby+zab*ubz;

	double vbx=xac*rrac-xad*rrad;
	double vby=yac*rrac-yad*rrad;
	double vbz=zac*rrac-zad*rrad;
	double vbn=1.0/sqrt(vbx*vbx+vby*vby+vbz*vbz);
	vbx=vbn*vbx;
	vby=vbn*vby;
	vbz=vbn*vbz;
	double rvb=xab*vbx+yab*vby+zab*vbz;
	double wwb=sqrt(rub*rub+rvb*rvb);

	double ucx=xad*rrad+xab*rrab;
	double ucy=yad*rrad+yab*rrab;
	double ucz=zad*rrad+zab*rrab;
	double ucn=1.0/sqrt(ucx*ucx+ucy*ucy+ucz*ucz);
	ucx=ucn*ucx;
	ucy=ucn*ucy;
	ucz=ucn*ucz;
	double ruc=xac*ucx+yac*ucy+zac*ucz;

	double vcx=xad*rrad-xab*rrab;
	double vcy=yad*rrad-yab*rrab;
	double vcz=zad*rrad-zab*rrab;
	double vcn=1.0/sqrt(vcx*vcx+vcy*vcy+vcz*vcz);
	vcx=vcn*vcx;
	vcy=vcn*vcy;
	vcz=vcn*vcz;
	double rvc=xac*vcx+yac*vcy+zac*vcz;
	double wwc=sqrt(ruc*ruc+rvc*rvc);

	double udx=xab*rrab+xac*rrac;
	double udy=yab*rrab+yac*rrac;
	double udz=zab*rrab+zac*rrac;
	double udn=1.0/sqrt(udx*udx+udy*udy+udz*udz);
	udx=udn*udx;
	udy=udn*udy;
	udz=udn*udz;
	double rud=xad*udx+yad*udy+zad*udz;

	double vdx=xab*rrab-xac*rrac;
	double vdy=yab*rrab-yac*rrac;
	double vdz=zab*rrab-zac*rrac;
	double vdn=1.0/sqrt(vdx*vdx+vdy*vdy+vdz*vdz);
	vdx=vdn*vdx;
	vdy=vdn*vdy;
	vdz=vdn*vdz;
	double rvd=xad*vdx+yad*vdy+zad*vdz;
	double wwd=sqrt(rud*rud+rvd*rvd);

	// calculate inversion angle cosines

	double cosb=wwb*rrab;
	double cosc=wwc*rrac;
	double cosd=wwd*rrad;

	// //////////////////////////////////////////////////////
	// planar inversion potentials

	u=A*(1.0-(cosb+cosc+cosd)/3.0);
	double gamb=A/3.0;
	double gamc=A/3.0;
	double gamd=A/3.0;
	// /////////////////////////////////////////////////////

	// calculate bond and u,v scalar products

	double rubc=xab*ucx+yab*ucy+zab*ucz;
	double rubd=xab*udx+yab*udy+zab*udz;
	double rucd=xac*udx+yac*udy+zac*udz;
	double rucb=xac*ubx+yac*uby+zac*ubz;
	double rudb=xad*ubx+yad*uby+zad*ubz;
	double rudc=xad*ucx+yad*ucy+zad*ucz;

	double rvbc=xab*vcx+yab*vcy+zab*vcz;
	double rvbd=xab*vdx+yab*vdy+zab*vdz;
	double rvcd=xac*vdx+yac*vdy+zac*vdz;
	double rvcb=xac*vbx+yac*vby+zac*vbz;
	double rvdb=xad*vbx+yad*vby+zad*vbz;
	double rvdc=xad*vcx+yad*vcy+zad*vcz;

	// calculate atomic forces

	fbx = gamb*(-cosb*xab*rrab*rrab+rrab*(rub*ubx+rvb*vbx)/wwb) +       
	( ruc*ucn*rrab*(xac-ruc*ucx-(rbc-ruc*rubc)*xab*rrab*rrab) -   
	rvc*vcn*rrab*(xac-rvc*vcx-(rbc-rvc*rvbc)*xab*rrab*rrab) ) * 
	gamc*rrac/wwc +                                             
	( rud*udn*rrab*(xad-rud*udx-(rdb-rud*rubd)*xab*rrab*rrab) +   
	rvd*vdn*rrab*(xad-rvd*vdx-(rdb-rvd*rvbd)*xab*rrab*rrab) ) * 
	gamd*rrad/wwd;

	fby = gamb*(-cosb*yab*rrab*rrab+rrab*(rub*uby+rvb*vby)/wwb) +       
	( ruc*ucn*rrab*(yac-ruc*ucy-(rbc-ruc*rubc)*yab*rrab*rrab) -   
	rvc*vcn*rrab*(yac-rvc*vcy-(rbc-rvc*rvbc)*yab*rrab*rrab) ) * 
	gamc*rrac/wwc +                                            
	( rud*udn*rrab*(yad-rud*udy-(rdb-rud*rubd)*yab*rrab*rrab) +   
	rvd*vdn*rrab*(yad-rvd*vdy-(rdb-rvd*rvbd)*yab*rrab*rrab) ) * 
	gamd*rrad/wwd;

	fbz = gamb*(-cosb*zab*rrab*rrab+rrab*(rub*ubz+rvb*vbz)/wwb) +       
	( ruc*ucn*rrab*(zac-ruc*ucz-(rbc-ruc*rubc)*zab*rrab*rrab) -   
	rvc*vcn*rrab*(zac-rvc*vcz-(rbc-rvc*rvbc)*zab*rrab*rrab) ) * 
	gamc*rrac/wwc +                                             
	( rud*udn*rrab*(zad-rud*udz-(rdb-rud*rubd)*zab*rrab*rrab) +   
	rvd*vdn*rrab*(zad-rvd*vdz-(rdb-rvd*rvbd)*zab*rrab*rrab) ) * 
	gamd*rrad/wwd;

	fcx = gamc*(-cosc*xac*rrac*rrac+rrac*(ruc*ucx+rvc*vcx)/wwc) +       
	( rud*udn*rrac*(xad-rud*udx-(rcd-rud*rucd)*xac*rrac*rrac) -   
	rvd*vdn*rrac*(xad-rvd*vdx-(rcd-rvd*rvcd)*xac*rrac*rrac) ) * 
	gamd*rrad/wwd +                                             
	( rub*ubn*rrac*(xab-rub*ubx-(rbc-rub*rucb)*xac*rrac*rrac) +   
	rvb*vbn*rrac*(xab-rvb*vbx-(rbc-rvb*rvcb)*xac*rrac*rrac) ) * 
	gamb*rrab/wwb;

	fcy = gamc*(-cosc*yac*rrac*rrac+rrac*(ruc*ucy+rvc*vcy)/wwc) +       
	( rud*udn*rrac*(yad-rud*udy-(rcd-rud*rucd)*yac*rrac*rrac) -   
	rvd*vdn*rrac*(yad-rvd*vdy-(rcd-rvd*rvcd)*yac*rrac*rrac) ) * 
	gamd*rrad/wwd +                                             
	( rub*ubn*rrac*(yab-rub*uby-(rbc-rub*rucb)*yac*rrac*rrac) +   
	rvb*vbn*rrac*(yab-rvb*vby-(rbc-rvb*rvcb)*yac*rrac*rrac) ) * 
	gamb*rrab/wwb;

	fcz = gamc*(-cosc*zac*rrac*rrac+rrac*(ruc*ucz+rvc*vcz)/wwc) +       
	( rud*udn*rrac*(zad-rud*udz-(rcd-rud*rucd)*zac*rrac*rrac) -   
	rvd*vdn*rrac*(zad-rvd*vdz-(rcd-rvd*rvcd)*zac*rrac*rrac) ) * 
	gamd*rrad/wwd +                                             
	( rub*ubn*rrac*(zab-rub*ubz-(rbc-rub*rucb)*zac*rrac*rrac) +   
	rvb*vbn*rrac*(zab-rvb*vbz-(rbc-rvb*rvcb)*zac*rrac*rrac) ) * 
	gamb*rrab/wwb;

	fdx = gamd*(-cosd*xad*rrad*rrad+rrad*(rud*udx+rvd*vdx)/wwd) +       
	( rub*ubn*rrad*(xab-rub*ubx-(rdb-rub*rudb)*xad*rrad*rrad) -   
	rvb*vbn*rrad*(xab-rvb*vbx-(rdb-rvb*rvdb)*xad*rrad*rrad) ) * 
	gamb*rrab/wwb +                                             
	( ruc*ucn*rrad*(xac-ruc*ucx-(rcd-ruc*rudc)*xad*rrad*rrad) +   
	rvc*vcn*rrad*(xac-rvc*vcx-(rcd-rvc*rvdc)*xad*rrad*rrad) ) * 
	gamc*rrac/wwc;

	fdy = gamd*(-cosd*yad*rrad*rrad+rrad*(rud*udy+rvd*vdy)/wwd) +       
	( rub*ubn*rrad*(yab-rub*uby-(rdb-rub*rudb)*yad*rrad*rrad) -   
	rvb*vbn*rrad*(yab-rvb*vby-(rdb-rvb*rvdb)*yad*rrad*rrad) ) * 
	gamb*rrab/wwb +                                             
	( ruc*ucn*rrad*(yac-ruc*ucy-(rcd-ruc*rudc)*yad*rrad*rrad) +   
	rvc*vcn*rrad*(yac-rvc*vcy-(rcd-rvc*rvdc)*yad*rrad*rrad) ) * 
	gamc*rrac/wwc;

	fdz = gamd*(-cosd*zad*rrad*rrad+rrad*(rud*udz+rvd*vdz)/wwd) +       
	( rub*ubn*rrad*(zab-rub*ubz-(rdb-rub*rudb)*zad*rrad*rrad) -   
	rvb*vbn*rrad*(zab-rvb*vbz-(rdb-rvb*rvdb)*zad*rrad*rrad) ) * 
	gamb*rrab/wwb +                                             
	( ruc*ucn*rrad*(zac-ruc*ucz-(rcd-ruc*rudc)*zad*rrad*rrad) +   
	rvc*vcn*rrad*(zac-rvc*vcz-(rcd-rvc*rvdc)*zad*rrad*rrad) ) * 
	gamc*rrac/wwc;

	fax = -(fbx+fcx+fdx);
	fay = -(fby+fcy+fdy);
	faz = -(fbz+fcz+fdz);
}
void inversion_plan_pdiff(double A,
double xa,double ya,double za,
double xb,double yb,double zb,
double xc,double yc,double zc,
double xd,double yd,double zd,
double & u,
double & dA
) 
{

	double xab= xb - xa;
	double yab= yb - ya;
	double zab= zb - za;
	double rab2=xab*xab+yab*yab+zab*zab;
	double rrab=1.0/sqrt(rab2);

	double xac= xc - xa;
	double yac= yc - ya;
	double zac= zc - za;
	double rac2=xac*xac+yac*yac+zac*zac;
	double rrac=1.0/sqrt(rac2);

	double xad= xd - xa;
	double yad= yd - ya;
	double zad= zd - za;
	double rad2=xad*xad+yad*yad+zad*zad;
	double rrad=1.0/sqrt(rad2);

	double rbc=xab*xac+yab*yac+zab*zac;
	double rcd=xac*xad+yac*yad+zac*zad;
	double rdb=xad*xab+yad*yab+zad*zab;

	// calculate bond-angle-plane vectors

	double ubx=xac*rrac+xad*rrad;
	double uby=yac*rrac+yad*rrad;
	double ubz=zac*rrac+zad*rrad;
	double ubn=1.0/sqrt(ubx*ubx+uby*uby+ubz*ubz);
	ubx=ubn*ubx;
	uby=ubn*uby;
	ubz=ubn*ubz;
	double rub=xab*ubx+yab*uby+zab*ubz;

	double vbx=xac*rrac-xad*rrad;
	double vby=yac*rrac-yad*rrad;
	double vbz=zac*rrac-zad*rrad;
	double vbn=1.0/sqrt(vbx*vbx+vby*vby+vbz*vbz);
	vbx=vbn*vbx;
	vby=vbn*vby;
	vbz=vbn*vbz;
	double rvb=xab*vbx+yab*vby+zab*vbz;
	double wwb=sqrt(rub*rub+rvb*rvb);

	double ucx=xad*rrad+xab*rrab;
	double ucy=yad*rrad+yab*rrab;
	double ucz=zad*rrad+zab*rrab;
	double ucn=1.0/sqrt(ucx*ucx+ucy*ucy+ucz*ucz);
	ucx=ucn*ucx;
	ucy=ucn*ucy;
	ucz=ucn*ucz;
	double ruc=xac*ucx+yac*ucy+zac*ucz;

	double vcx=xad*rrad-xab*rrab;
	double vcy=yad*rrad-yab*rrab;
	double vcz=zad*rrad-zab*rrab;
	double vcn=1.0/sqrt(vcx*vcx+vcy*vcy+vcz*vcz);
	vcx=vcn*vcx;
	vcy=vcn*vcy;
	vcz=vcn*vcz;
	double rvc=xac*vcx+yac*vcy+zac*vcz;
	double wwc=sqrt(ruc*ruc+rvc*rvc);

	double udx=xab*rrab+xac*rrac;
	double udy=yab*rrab+yac*rrac;
	double udz=zab*rrab+zac*rrac;
	double udn=1.0/sqrt(udx*udx+udy*udy+udz*udz);
	udx=udn*udx;
	udy=udn*udy;
	udz=udn*udz;
	double rud=xad*udx+yad*udy+zad*udz;

	double vdx=xab*rrab-xac*rrac;
	double vdy=yab*rrab-yac*rrac;
	double vdz=zab*rrab-zac*rrac;
	double vdn=1.0/sqrt(vdx*vdx+vdy*vdy+vdz*vdz);
	vdx=vdn*vdx;
	vdy=vdn*vdy;
	vdz=vdn*vdz;
	double rvd=xad*vdx+yad*vdy+zad*vdz;
	double wwd=sqrt(rud*rud+rvd*rvd);

	// calculate inversion angle cosines

	double cosb=wwb*rrab;
	double cosc=wwc*rrac;
	double cosd=wwd*rrad;

	// //////////////////////////////////////////////////////
	// planar inversion potentials

	u=A*(1.0-(cosb+cosc+cosd)/3.0);
	dA=1.0-(cosb+cosc+cosd)/3.0;
	
	// /////////////////////////////////////////////////////

}

void inversion::plan::eval(real_type & u,vector_array_type & f,const vector_array_type & r) const
{
	const Eigen::Vector3d & ri = r[i];
	const Eigen::Vector3d & rj = r[j];
	const Eigen::Vector3d & rk = r[k];
	const Eigen::Vector3d & rl = r[l];
	Eigen::Vector3d fi;
	Eigen::Vector3d fj;
	Eigen::Vector3d fk;
	Eigen::Vector3d fl;

	real_type uijkl = 0.0;
	inversion_plan_force(p_A,
	ri.x(),ri.y(),ri.z(),
	rj.x(),rj.y(),rj.z(),
	rk.x(),rk.y(),rk.z(),
	rl.x(),rl.y(),rl.z(),

	uijkl,

	fi.x(),fi.y(),fi.z(),
	fj.x(),fj.y(),fj.z(),
	fk.x(),fk.y(),fk.z(),
	fl.x(),fl.y(),fl.z() );
 
	u += uijkl;
	f[i] += fi;
	f[j] += fj;
	f[k] += fk;
	f[l] += fl;
	
}
void inversion::plan_fit::peval(real_type & u,vectorx_type & dp,const vectorx_type & p,const vector_array_type & r)
{
	if(i_A>=0)		p_A    = p(i_A);

	const Eigen::Vector3d & ri = r[i];
	const Eigen::Vector3d & rj = r[j];
	const Eigen::Vector3d & rk = r[k];
	const Eigen::Vector3d & rl = r[l];
	Eigen::Vector3d fi;
	Eigen::Vector3d fj;
	Eigen::Vector3d fk;
	Eigen::Vector3d fl;

	real_type uijkl = 0.0;
	real_type dA = 0.0;
	inversion_plan_pdiff(p_A,
	ri.x(),ri.y(),ri.z(),
	rj.x(),rj.y(),rj.z(),
	rk.x(),rk.y(),rk.z(),
	rl.x(),rl.y(),rl.z(),

	uijkl,

	dA );
 
	u += uijkl;

	if(i_A>=0)    dp(i_A)    += dA;

}








void dihedral_cos_force(double A,double delta,double m,

double xa,double ya,double za,
double xb,double yb,double zb,
double xc,double yc,double zc,
double xd,double yd,double zd,

double & u,

double &fax,double &fay,double &faz,
double &fbx,double &fby,double &fbz,
double &fcx,double &fcy,double &fcz,
double &fdx,double &fdy,double &fdz			
) 
{
	double xab= xb - xa;
	double yab= yb - ya;
	double zab= zb - za;

	double xbc= xc - xb;
	double ybc= yc - yb;
	double zbc= zc - zb;
	double rrbc=1.0/sqrt(xbc*xbc+ybc*ybc+zbc*zbc);

	double xcd= xd - xc;
	double ycd= yd - yc;
	double zcd= zd - zc;

	double xac=xab+xbc;
	double yac=yab+ybc;
	double zac=zab+zbc;

	// construct first dihedral vector

	double pbx=yab*zbc-zab*ybc;
	double pby=zab*xbc-xab*zbc;
	double pbz=xab*ybc-yab*xbc;

	double pb2=pbx*pbx+pby*pby+pbz*pbz;

	double rpb1=1.0/sqrt(pb2);
	double rpb2=rpb1*rpb1;

	// construct second dihedral vector

	double pcx=ybc*zcd-zbc*ycd;
	double pcy=zbc*xcd-xbc*zcd;
	double pcz=xbc*ycd-ybc*xcd;

	double pc2=pcx*pcx+pcy*pcy+pcz*pcz;

	double rpc1=1.0/sqrt(pc2);
	double rpc2=rpc1*rpc1;

	// determine dihedral angle

	double pbpc=pbx*pcx+pby*pcy+pbz*pcz;
	double cost=pbpc*rpb1*rpc1;
	double sint=( xbc*(pcy*pbz-pcz*pby) + ybc*(pbx*pcz-pbz*pcx) + 
		   zbc*(pcx*pby-pcy*pbx) ) * (rpb1*rpc1*rrbc);

	double theta=atan2(sint,cost);

	// avoid singularity in sint

	//!!!			sint=sign( std::max(1.0e-8,std:abs(sint)) , sint );
	double rsint=1.0/sint;

	double term=m*theta-delta;

	u = A*(1.0+cos(term));
	double gamma=-A*m*std::sin(term)*rsint * rpb1*rpc1;

	fax = gamma*((-pcy*zbc+pcz*ybc)-pbpc*rpb2*(-pby*zbc+pbz*ybc));
	fay = gamma*(( pcx*zbc-pcz*xbc)-pbpc*rpb2*( pbx*zbc-pbz*xbc));
	faz = gamma*((-pcx*ybc+pcy*xbc)-pbpc*rpb2*(-pbx*ybc+pby*xbc));

	fcx = gamma*((-pcy*zab+pcz*yab)-pbpc*rpb2*(-pby*zab+pbz*yab));
	fcy = gamma*(( pcx*zab-pcz*xab)-pbpc*rpb2*( pbx*zab-pbz*xab));
	fcz = gamma*((-pcx*yab+pcy*xab)-pbpc*rpb2*(-pbx*yab+pby*xab));

	double fb1x= gamma*((-pby*zcd+pbz*ycd)-pbpc*rpc2*(-pcy*zcd+pcz*ycd));
	double fb1y= gamma*(( pbx*zcd-pbz*xcd)-pbpc*rpc2*( pcx*zcd-pcz*xcd));
	double fb1z= gamma*((-pbx*ycd+pby*xcd)-pbpc*rpc2*(-pcx*ycd+pcy*xcd));

	double fd1x= gamma*((-pby*zbc+pbz*ybc)-pbpc*rpc2*(-pcy*zbc+pcz*ybc));
	double fd1y= gamma*(( pbx*zbc-pbz*xbc)-pbpc*rpc2*( pcx*zbc-pcz*xbc));
	double fd1z= gamma*((-pbx*ybc+pby*xbc)-pbpc*rpc2*(-pcx*ybc+pcy*xbc));

	fbx = -fax-fcx+fb1x;
	fby = -fay-fcy+fb1y;
	fbz = -faz-fcz+fb1z;

	fcx = +fcx-fb1x-fd1x;
	fcy = +fcy-fb1y-fd1y;
	fcz = +fcz-fb1z-fd1z;

	fdx = fd1x;
	fdy = fd1y;
	fdz = fd1z;

    //faxx=fax;
    //fayy=fay;
    //fazz=faz;

	//fbxx=-fax-fcx+fb1x;
	//fbyy=-fay-fcy+fb1y;
	//fbzz=-faz-fcz+fb1z;

	fcx=+fcx-fb1x-fd1x;
	fcy=+fcy-fb1y-fd1y;
	fcz=+fcz-fb1z-fd1z;

	//fdxx=+fd1x;
	//fdyy=+fd1y;
	//fdzz=+fd1z;

}

void dihedral_cos_pdiff(double A,double delta,double m,

double xa,double ya,double za,
double xb,double yb,double zb,
double xc,double yc,double zc,
double xd,double yd,double zd,

double & u,

double & dA,double & ddelta,double &dm
) 
{
	double xab= xb - xa;
	double yab= yb - ya;
	double zab= zb - za;

	double xbc= xc - xb;
	double ybc= yc - yb;
	double zbc= zc - zb;
	double rrbc=1.0/sqrt(xbc*xbc+ybc*ybc+zbc*zbc);

	double xcd= xd - xc;
	double ycd= yd - yc;
	double zcd= zd - zc;

	double xac=xab+xbc;
	double yac=yab+ybc;
	double zac=zab+zbc;

	// construct first dihedral vector

	double pbx=yab*zbc-zab*ybc;
	double pby=zab*xbc-xab*zbc;
	double pbz=xab*ybc-yab*xbc;

	double pb2=pbx*pbx+pby*pby+pbz*pbz;

	double rpb1=1.0/sqrt(pb2);
	double rpb2=rpb1*rpb1;

	// construct second dihedral vector

	double pcx=ybc*zcd-zbc*ycd;
	double pcy=zbc*xcd-xbc*zcd;
	double pcz=xbc*ycd-ybc*xcd;

	double pc2=pcx*pcx+pcy*pcy+pcz*pcz;

	double rpc1=1.0/sqrt(pc2);
	double rpc2=rpc1*rpc1;

	// determine dihedral angle

	double pbpc=pbx*pcx+pby*pcy+pbz*pcz;
	double cost=pbpc*rpb1*rpc1;
	double sint=( xbc*(pcy*pbz-pcz*pby) + ybc*(pbx*pcz-pbz*pcx) + 
		   zbc*(pcx*pby-pcy*pbx) ) * (rpb1*rpc1*rrbc);

	double theta=atan2(sint,cost);

	// avoid singularity in sint

	//!!!			sint=sign( std::max(1.0e-8,std:abs(sint)) , sint );
	double rsint=1.0/sint;

	double term=m*theta-delta;

	u = A*(1.0+cos(term));
	dA = 1.0+cos(term);
	ddelta = A*sin(term);
	dm = -A*theta*sin(term);
}



/*
void dihedral::cosf::eval(real_type & u,vector_array_type & f,const vector_array_type & r) const
{
	const Eigen::Vector3d & ri = r[i];
	const Eigen::Vector3d & rj = r[j];
	const Eigen::Vector3d & rk = r[k];
	const Eigen::Vector3d & rl = r[l];
	Eigen::Vector3d fi;
	Eigen::Vector3d fj;
	Eigen::Vector3d fk;
	Eigen::Vector3d fl;

	real_type uijkl = 0.0;
	dihedral_cos_force(p_A,p_d,p_m,
	ri.x(),ri.y(),ri.z(),
	rj.x(),rj.y(),rj.z(),
	rk.x(),rk.y(),rk.z(),
	rl.x(),rl.y(),rl.z(),

	uijkl,

	fi.x(),fi.y(),fi.z(),
	fj.x(),fj.y(),fj.z(),
	fk.x(),fk.y(),fk.z(),
	fl.x(),fl.y(),fl.z() );
 
	u += uijkl;
	f[i] += fi;
	f[j] += fj;
	f[k] += fk;
	f[l] += fl;
	
}


void dihedral::cosf_fit::peval(real_type & u,vectorx_type & dp,const vectorx_type & p,const vector_array_type & r)
{
	if(i_A>=0)		p_A    = p(i_A);
	if(i_d>=0)      p_d    = p(i_d);
	if(i_m>=0)      p_m    = p(i_m);

	const Eigen::Vector3d & ri = r[i];
	const Eigen::Vector3d & rj = r[j];
	const Eigen::Vector3d & rk = r[k];
	const Eigen::Vector3d & rl = r[l];
	Eigen::Vector3d fi;
	Eigen::Vector3d fj;
	Eigen::Vector3d fk;
	Eigen::Vector3d fl;

	real_type uijkl = 0.0;
	real_type dA = 0.0;
	real_type ddelta = 0.0;
	real_type dm = 0.0;
	dihedral_cos_pdiff(p_A,p_d,p_m,
	ri.x(),ri.y(),ri.z(),
	rj.x(),rj.y(),rj.z(),
	rk.x(),rk.y(),rk.z(),
	rl.x(),rl.y(),rl.z(),

	uijkl,

	dA,ddelta,dm );

	u+= uijkl;

	if(i_A>=0)    dp(i_A)    += dA;
	if(i_d>=0)    dp(i_d)    += ddelta;
	if(i_m>=0)    dp(i_m)    += dm;
}

*/
void dihedral::cosf::eval(real_type & u,vector_array_type & f,const vector_array_type & r) const
{
    vector_type a = r[i] - r[j];  vector_type b = r[j] - r[k];  vector_type c = r[k] - r[l];

    real_type aa = a.dot(a);  real_type bb = b.dot(b);  real_type cc = c.dot(c);
    real_type ab = a.dot(b);  real_type ac = a.dot(c);  real_type bc = b.dot(c);
    
    vector_type axb   = a.cross(b);       vector_type bxc   = b.cross(c); 

    real_type   ilaxb = 1.0/axb.norm();   real_type   ilbxc = 1.0/bxc.norm();
    
    //axbbxc = axb*bxc;
    real_type axbbxc = ab * bc - bb * ac;
    
    real_type inv = ilaxb*ilbxc;
    
    vector_type Aaxbbxc = b * bc - bb * c;
    vector_type Baxbbxc = a * bc + ab * c - 2.0 * b * ac;
    vector_type Caxbbxc = ab * b - bb * a;
    
    vector_type Alaxb = ilaxb * ( bb * a - ab * b );
    vector_type Blaxb = ilaxb * ( aa * b - ab * a );

    vector_type Blbxc = ilbxc * ( cc * b - bc * c );
    vector_type Clbxc = ilbxc * ( bb * c - bc * b );

    vector_type Ailaxb = - ilaxb*ilaxb * Alaxb;
    vector_type Bilaxb = - ilaxb*ilaxb * Blaxb;    
    
    vector_type Bilbxc = - ilbxc*ilbxc * Blbxc;
    vector_type Cilbxc = - ilbxc*ilbxc * Clbxc;
        
    vector_type Ainv = ilbxc * Ailaxb;
    vector_type Binv = Bilaxb * ilbxc + ilaxb * Bilbxc;
    vector_type Cinv = ilaxb * Cilbxc;
            
    real_type cosphi = axbbxc*inv;
    
    vector_type Acosphi = Aaxbbxc * inv + axbbxc * Ainv;
    vector_type Bcosphi = Baxbbxc * inv + axbbxc * Binv;
    vector_type Ccosphi = Caxbbxc * inv + axbbxc * Cinv;
    
    real_type phi = acos(cosphi);
    real_type isinphi = 1.0/sin(phi);
 
	/////////////
    u += p_A*(1.0+cos(p_m*phi-p_d));
    real_type du = -p_A*p_m*sin(p_m*phi-p_d);
    /////////////

    vector_type fi = isinphi * du * Acosphi;
    vector_type fj = isinphi * du * (Bcosphi - Acosphi);
    vector_type fl = -isinphi * du * Ccosphi;
    vector_type fk = -fi-fj-fl;

	f[i] += fi;
	f[j] += fj;
	f[k] += fk;
	f[l] += fl;

}
	

void dihedral::cosf_fit::peval(real_type & u,vectorx_type & dp,const vectorx_type & p,const vector_array_type & r)
{
	if(i_A>=0)		p_A    = p(i_A);
	if(i_d>=0)      p_d    = p(i_d);
	if(i_m>=0)      p_m    = p(i_m);

	real_type   cosphi = (r[i] - r[j]).cross(r[j] - r[k]).normalized().dot((r[j] - r[k]).cross(r[k] - r[l]).normalized());
	real_type   phi = std::acos(cosphi);

	u += p_A*(1.0+cos(p_m*phi-p_d));

	if(i_A>=0)    dp(i_A)    += 1.0+cos(p_m*phi-p_d);
	if(i_d>=0)    dp(i_d)    += p_A*sin(p_m*phi-p_d);
	if(i_m>=0)    dp(i_m)    += -p_A*phi*sin(p_m*phi-p_d);
}
